/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    BalanceController.cpp
  * @author  Mentos Seetoo
  * @brief   A PD controller based on rigid body kinematic model is used to
  *          control body position and rotation.
  * @note    THe following third-party libraries are used:
  *          - Eigen3
  * @warning 
  *     - C++11 is required.												 
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "Motion_Controller/BalanceController.h"

using namespace Quadruped;
/* Founctions ----------------------------------------------------------------*/
template <typename T>
BalanceController<T>::BalanceController()
{
    /*Set init val*/
    w_d = Vec3<T>::Zero();
    p_error = Vec3<T>::Zero();
    pdot_error = Vec3<T>::Zero();
    ori_error = Vec3<T>::Zero();
    w_error = Vec3<T>::Zero();
    R_B = Mat3<T>::Ones();
    R_d = Mat3<T>::Ones();
    Kd_x = 0;
    Kd_y = 0;
    Kd_z = 0;
    Kp_x = 0;
    Kp_y = 0;
    Kp_z = 0;
    Kp_roll = 0;
    Kp_pitch = 0;
    Kp_yaw = 0;
    Kd_pitch = 0;
    Kd_roll = 0;
    Kd_yaw = 0;
}

template <typename T>
void BalanceController<T>::CalConrtollerOutput()
{ 
    //!<Update error
    p_error = p_d - p_c;
    pdot_error = v_d - v_c;
    matrixLogRot(R_B.transpose() * R_d, ori_error);
    w_error = w_d - w_c;

    //!< Cal Output, out_v is CoM velocity control value in {G} and 
    //!< out_w is CoM attitude control value in {G}
    out_v(0) = Kp_x * p_error(0) + Kd_x * pdot_error(0);
    out_v(1) = Kp_y * p_error(1) + Kd_y * pdot_error(1);
    out_v(2) = Kp_z * p_error(2) + Kd_z * pdot_error(2);

    out_w(0) = Kp_roll * ori_error(0) + Kd_roll * w_error(0);
    out_w(1) = Kp_pitch * ori_error(1) + Kd_pitch * w_error(1);
    out_w(2) = Kp_yaw * ori_error(2) + Kd_yaw * w_error(2);
}

template <typename T>
void BalanceController<T>::SetTargets(const Vec3<T> &p, const Vec3<T> &v, const Vec3<T> &w, const Mat3<T> &R)
{
    p_d = p;
    v_d = v;
    w_d = w;
    R_d = R;
}

template <typename T>
void BalanceController<T>::UpdateValues(const Vec3<T> &p, const Vec3<T> &v, const Vec3<T> &w, const Mat3<T> &R)
{
    p_c = p;
    v_c = v;
    w_c = w;
    R_B = R;
}

template <typename T>
void BalanceController<T>::matrixLogRot(const Mat3<T>& R, Vec3<T>& omega) {
  double theta;
  double tmp = (R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2;
  if (tmp >= 1.) {
    theta = 0;
  } else if (tmp <= -1.) {
    theta = M_PI;
  } else {
    theta = acos(tmp);
  }

  omega << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1);
  if (theta > 10e-5) {
    omega *= theta / (2 * sin(theta));
  } else {
    omega /= 2;
  }
}

template <typename T>
void BalanceController<T>::crossMatrix(Mat3<T>& R, const Vec3<T>& omega) {
  R(0, 1) = -omega(2);
  R(0, 2) = omega(1);
  R(1, 0) = omega(2);
  R(1, 2) = -omega(0);
  R(2, 0) = -omega(1);
  R(2, 1) = omega(0);
}

template <typename T>
void BalanceController<T>::SetPGains(T x, T y, T z, T roll, T pitch, T yaw)
{
    Kp_x = x;
    Kp_y = y;
    Kp_z = z;
    Kp_roll = roll;
    Kp_pitch = pitch;
    Kp_yaw = yaw;
}

template <typename T>
void BalanceController<T>::SetDGains(T x, T y, T z, T roll, T pitch, T yaw)
{
    Kd_x = x;
    Kd_y = y;
    Kd_z = z;
    Kd_roll = roll;
    Kd_pitch = pitch;
    Kd_yaw = yaw;
}

template class BalanceController<float>;
template class BalanceController<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/